/**
 * @author Yunfeng Bai Wo Hen ER
 *
 * 2012-11-30
 */
package collision;

import geometry.Drawable;
import geometry.Triangle;
import geometry.TriangleMesh;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;

import javax.media.opengl.GL2;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import sun.reflect.generics.reflectiveObjects.NotImplementedException;
import util.Timing;

public class OBBTreeNode extends BoundingVolume
{
	protected static final int MAX_TRIANGLE_COUNT_IN_LEAF = 2;
	protected static final int MAX_DEPTH = 15;
	protected OBB boundingBox;
	protected OBBTreeNode leftChild = null, rightChild = null;
	protected List<Triangle> triangles;
	protected boolean isLeaf = false;
	protected int level;
	protected int nodeCount = 0;
	protected OBBTree tree;
	
	public OBBTreeNode(OBBTree tree) { this.tree = tree; }
	
	/**
	 * Build OBB tree based on a list of triangles in MODEL SPACE.
	 * This routing will find the mid point of the widest axis first,
	 * then split triangles into two parts base on their centers.
	 */
	public void buildTree(int depth, List<Triangle> triangles)
	{
		level = depth;
		this.triangles = triangles;
		boundingBox = new OBB(triangles, tree);
		
		// Stop criteria.
		if (depth > MAX_DEPTH || triangles.size() <= MAX_TRIANGLE_COUNT_IN_LEAF)
		{
			nodeCount = 1;
			isLeaf = true;
			return;
		}
		
		// Find the widest axis.
		int axis = 0;
		double maxSize = -1;
		for (int i = 0; i < 3; i++)
			if (boundingBox.size[i] > maxSize)
			{
				maxSize = boundingBox.size[i];
				axis = i;
			}
		
		// Assign triangles to left or right leaves based on their centers.
		List<Triangle> leftTriangles = new ArrayList<Triangle>();
		List<Triangle> rightTriangles = new ArrayList<Triangle>();
		Vector3d a = boundingBox.axes[axis];
		Point3d c = boundingBox.center;
		double mid = a.x * c.x + a.y * c.y + a.z * c.z;
		for (Triangle t : triangles)
		{
			c = t.getCenter();
			double v = a.x * c.x + a.y * c.y + a.z * c.z;
			if (v < mid) leftTriangles.add(t);
			else rightTriangles.add(t);
		}
		
		if (leftTriangles.isEmpty() || rightTriangles.isEmpty())
		{
			nodeCount = 1;
			isLeaf = true;
			return;
		}
		
		leftChild = new OBBTreeNode(tree);
		rightChild = new OBBTreeNode(tree);
		leftChild.buildTree(depth + 1, leftTriangles);
		rightChild.buildTree(depth + 1, rightTriangles);
		
		nodeCount += leftChild.nodeCount + rightChild.nodeCount;
	}
	
	public void collide(BoundingVolume obj_, HashSet<Triangle> triangles1, HashSet<Triangle> triangles2)
	{
		if (!(obj_ instanceof OBBTreeNode))
			throw new IllegalArgumentException("OBB Tree can only collide with OBB Tree");
		
		collideInternal((OBBTreeNode)obj_, triangles1, triangles2);
	}
	
	/**
	 * Collide this tree with another OBB tree, storing all potentially colliding triangles
	 * in this tree in triangles1, and all potentially colliding triangles in the other tree
	 * in triangles2.
	 * Consider four case:
	 * 1. Both are leaves, detect collision between all pairs of triangles.
	 * 2. This node is a leaf, the other one is not, descend the other node.
	 * 3. This node is not a leaf, the other one is, descend this node.
	 * 4. This node is not a leaf, the other one is not either, descend both with 4 possibilities.
	 */
	private void collideInternal(OBBTreeNode tree, HashSet<Triangle> triangles1, HashSet<Triangle> triangles2)
	{
		boundingBox.update();
		tree.getOBB().update();
		
		long time1;
		
		time1 = System.nanoTime();
		boolean isColliding = boundingBox.isColliding(tree.getOBB());
		Timing.OBBCollisionTime += System.nanoTime() - time1;
		Timing.OBBCollisionCount++;
		
		if (isColliding == false)
		{
			return;
		}
		else if (isLeaf)
		{
			if (tree.isLeaf)
			{
				// Actual penetration detections are handled to ASDF trees.
				for (Triangle t1 : triangles)
					triangles1.add(t1);
				for (Triangle t2 : tree.triangles)
					triangles2.add(t2);
				return;
			}
			else
			{
				collideInternal(tree.leftChild, triangles1, triangles2);
				collideInternal(tree.rightChild, triangles1, triangles2);
			}
		}
		else if (tree.isLeaf)
		{
			leftChild.collideInternal(tree, triangles1, triangles2);
			rightChild.collideInternal(tree, triangles1, triangles2);
		}
		else // if (!isLeaf && !tree.isLeaf)
		{
			leftChild.collideInternal(tree.leftChild, triangles1, triangles2);
			leftChild.collideInternal(tree.rightChild, triangles1, triangles2);
			rightChild.collideInternal(tree.leftChild, triangles1, triangles2);
			rightChild.collideInternal(tree.rightChild, triangles1, triangles2);
		}
	}
	
	@Override
	public boolean isColliding(BoundingVolume obj_)
	{
		if (!(obj_ instanceof OBBTreeNode))
			throw new IllegalArgumentException("OBBTreeNode can only collide with OBBTreeNode");

		return isCollidingInternal((OBBTreeNode)obj_);
	}
	
	private boolean isCollidingInternal(OBBTreeNode tree)
	{
		boundingBox.update();
		tree.getOBB().update();
		
		if (boundingBox.isColliding(tree.getOBB()) == false)
		{
			return false;
		}
		else if (isLeaf)
		{
			if (tree.isLeaf)
			{
				// (TODO) Change to use ASDF to check penetration.
				Matrix4d transformMatrix = new Matrix4d(tree.boundingBox.getToWorldSpaceMatrix());
				transformMatrix.mul(boundingBox.getToModelSpaceMatrix(), transformMatrix);
				for (Triangle t1 : triangles)
				{
					for (Triangle t2 : tree.triangles)
					{
						if (t1.hasIntersectionWithTriangle(t2, transformMatrix)) return true;
					}
				}
				return false;
			}
			else
			{
				return isCollidingInternal(tree.leftChild)
					|| isCollidingInternal(tree.rightChild);
			}
		}
		else if (tree.isLeaf)
		{
			return leftChild.isCollidingInternal(tree)
				|| rightChild.isCollidingInternal(tree);
		}
		else // if (!isLeaf && !tree.isLeaf)
		{
			return leftChild.isCollidingInternal(tree.leftChild)
				|| leftChild.isCollidingInternal(tree.rightChild)
				|| rightChild.isCollidingInternal(tree.leftChild)
				|| rightChild.isCollidingInternal(tree.rightChild);
		}
	}
	
	private boolean isInside(Point3d point, Triangle.ClosestPointInfo info)
	{
		Triangle t = info.triangle;
		
		Vector3d v = new Vector3d(point);
		v.sub(info.targetPoint);
		if (info.type == 0)
		{
			return (t.getNormal().dot(v) < 0);
		}
		else if (info.type >= 1 && info.type <= 3)
		{
			return (t.getNormalOnEdges(info.type).dot(v) < 0);
		}
		else if (info.type >= 4 && info.type <= 6)
		{
			return (t.getNormalOnVertices(info.type).dot(v) < 0);
		}
		throw new NotImplementedException();
	}
	
	public double getUnsignedDistance(Point3d point)
	{
		Triangle.ClosestPointInfo info = getClosestPoint(point);
		
		return Math.sqrt(info.distanceSqr);
	}
	
	public double getSignedDistance(Point3d point)
	{
		Triangle.ClosestPointInfo info = getClosestPoint(point);
		
		double sign;
		if (isInside(point, info))
			sign = -1;
		else
			sign = +1;
		
		return Math.sqrt(info.distanceSqr) * sign;
	}
	
	public boolean isIntersectedWithOBB(OBB obb)
	{
		if (!boundingBox.isColliding(obb)) return false;
		
		if (isLeaf) return true;
		
		return leftChild.isIntersectedWithOBB(obb)
			|| rightChild.isIntersectedWithOBB(obb);
	}
	
	public Triangle.ClosestPointInfo getClosestPoint(Point3d point)
	{
		Triangle.ClosestPointInfo info = new Triangle.ClosestPointInfo();
		info.distanceSqr = Double.POSITIVE_INFINITY;
		getClosestPointInternal(point, info);
		return info;
	}
	
	public void getClosestPointInternal(Point3d point, Triangle.ClosestPointInfo info)
	{
		double dist = boundingBox.manhattanDistanceToPoint(point);
		if (dist > 0)
		{
			if (dist * dist >= info.distanceSqr)
				return;
		}
	
		if (isLeaf)
		{
			for (int it = 0; it < triangles.size(); it++)
			{
				Triangle t = triangles.get(it);
				Triangle.ClosestPointInfo i = t.getClosestPoint(point);
				if (i.distanceSqr < info.distanceSqr)
				{
					info.distanceSqr = i.distanceSqr;
					info.targetPoint = i.targetPoint;
					info.type = i.type;
					info.triangle = t;
				}
			}
			return;
		}
		
		leftChild.getClosestPointInternal(point, info);
		rightChild.getClosestPointInternal(point, info);
	}
	
	@Override
	public void build(GL2 gl)
	{
		_listNum = gl.glGenLists(1);
		gl.glNewList(_listNum, GL2.GL_COMPILE);
		
		gl.glPolygonMode(GL2.GL_FRONT_AND_BACK, GL2.GL_LINE);
		gl.glDisable(GL2.GL_CULL_FACE);
		
		buildTriangleMesh(gl);
		
		gl.glEnable(GL2.GL_CULL_FACE);
		gl.glPolygonMode(GL2.GL_FRONT_AND_BACK, GL2.GL_FILL);
		
		gl.glEndList();
	}
	
	protected void buildTriangleMesh(GL2 gl)
	{
		if (isLeaf)
		{
			boundingBox.directDraw(gl);
			return;
		}
		
		leftChild.buildTriangleMesh(gl);
		rightChild.buildTriangleMesh(gl);
	}
	
	@Override
	public TriangleMesh getTriangleMesh()
	{
		throw new NotImplementedException();
	}

	@Override
	public BoundingBox getBoundingBox()
	{
		// No need to get bounding box here.
		throw new NotImplementedException();
	}
	
	public OBB getOBB()
	{
		return boundingBox;
	}
}
